#include "headfile.h"

float zero_gz = 0.0f; 

void filter_init(void)
{
    int32_t totle_gz = 0;
    int cnt = 300;
    for (int i = 0; i < cnt; )
    {
        MPU6050_GetData();
        if (gz > 100 || gz < -100)
            continue;
        totle_gz += gz;
        ++i;
    }
    zero_gz = totle_gz / (float)cnt;
    char str[20];
    sprintf(str,"gyro OK:%.2f", zero_gz);
    OLED_ShowString(1, 1, str);
    delay_s(1);
    OLED_ShowString(1,1,"                 ");
}
